// Example of using the PVision library for interaction with the Pixart sensor on a WiiMote
// This work was derived from Kako's excellent Japanese website
// http://www.kako.com/neta/2007-001/2007-001.html

// Steve Hobley 2009 - www.stephenhobley.com
/******************************************************************************
* Includes
******************************************************************************/
#include "PVision.h"
#include <Wire.h>

/******************************************************************************
* Private methods
******************************************************************************/
void PVision::Write_2bytes(byte d1, byte d2)
{
    Wire.beginTransmission(IRslaveAddress);
    Wire.write(d1); Wire.write(d2);
    Wire.endTransmission();
}

void PVision::Write_nbytes(byte numbytes, byte configbyte[])
{
    Wire.beginTransmission(IRslaveAddress);
	for(int x = 0; x < numbytes; x++){
		Wire.write(configbyte[x]);
	}
    Wire.endTransmission();
}

/******************************************************************************
* Constructor
******************************************************************************/
PVision::PVision()
{
	Blob1.number = 1;
	Blob2.number = 2;
	Blob3.number = 3;
	Blob4.number = 4;
}

/******************************************************************************
* Public methods
******************************************************************************/
// init the PVision sensor
void PVision::init (byte gain, byte minsize, byte maxsize, byte gainlim)
{
    IRsensorAddress = 0xB0;
    IRslaveAddress = IRsensorAddress >> 1;   // This results in 0x21 as the address to pass to TWI

    Wire.begin();
	
    Write_2bytes(0x30,0x01); delay(10); //Control byte, allows modification of settings
    Write_2bytes(0x06,maxsize); delay(10); // 10 MAXSIZE - Maximum blob size. Wii uses values from 0x62 to 0xc8 (98 to 200).
    Write_2bytes(0x08,gain); delay(10); // 15 GAIN - Sensor Gain. Smaller values = higher gain. Numerical gain is proportional to 1/2^(n/16) for n<0x40
    Write_2bytes(0x1A,gainlim); delay(10); // 10 GAINLIMIT - Sensor Gain Limit. Must be less than GAIN for camera to function. No other effect?
    Write_2bytes(0x1B,minsize); delay(10); // * MINSIZE - Minimum blob size. Wii uses values from 3 to 5
    Write_2bytes(0x33,0x33); delay(10); // 
    Write_2bytes(0x30,0x08); delay(10); //Was Out of order, needs to be at end
    delay(100);

/*	
    // IR sensor initialize
    Write_2bytes(0x30,0x01); delay(10);
    Write_2bytes(0x30,0x08); delay(10);
    Write_2bytes(0x06,0x90); delay(10); //max blob size
    Write_2bytes(0x08,0xC0); delay(10); //sensor gain
    Write_2bytes(0x1A,0x40); delay(10); //gain limit
    Write_2bytes(0x33,0x33); delay(10);
    delay(100);  

/*
    Write_2bytes(0x30,0x01); delay(100);
	byte configbyte[] = {0x00, 0x02, 0x00, 0x00, 0x71, 0x01, 0x00, 0x96};
	Write_nbytes(8, configbyte); delay(100);
	byte configbyte2[] = {0x07, 0x00, 0xFE};
	Write_nbytes(3, configbyte2); delay(100);
	byte configbyte3[] = {0x07, 0xFE, 0x05};
	Write_nbytes(3, configbyte3); delay(100);
    Write_2bytes(0x33, 0x03); delay(100);
    Write_2bytes(0x30, 0x08); delay(100);
    delay(100);	*/
	
}

byte PVision::read()
{
    //IR sensor read
    Wire.beginTransmission(IRslaveAddress);
    Wire.write(0x36);
    Wire.endTransmission();

    Wire.requestFrom(IRslaveAddress, 16);        // Request the 2 byte heading (MSB comes first)
    for (i=0;i<16;i++)
    {
       data_buf[i]=0;
    }

    i=0;

    while(Wire.available() && i < 16)
    {
        data_buf[i] = Wire.read();
        i++;
    }

    blobcount = 0;

    Blob1.X = data_buf[1];
    Blob1.Y = data_buf[2];
    s   = data_buf[3];
    Blob1.X += (s & 0x30) <<4;
    Blob1.Y += (s & 0xC0) <<2;
    Blob1.Size = (s & 0x0F);

    // At the moment we're using the size of the blob to determine if one is detected, either X,Y, or size could be used.
    blobcount |= (Blob1.Size < 15)? BLOB1 : 0;

    Blob2.X = data_buf[4];
    Blob2.Y = data_buf[5];
    s   = data_buf[6];
    Blob2.X += (s & 0x30) <<4;
    Blob2.Y += (s & 0xC0) <<2;
    Blob2.Size = (s & 0x0F);

    blobcount |= (Blob2.Size < 15)? BLOB2 : 0;

    Blob3.X = data_buf[7];
    Blob3.Y = data_buf[8];
    s   = data_buf[9];
    Blob3.X += (s & 0x30) <<4;
    Blob3.Y += (s & 0xC0) <<2;
    Blob3.Size = (s & 0x0F);

    blobcount |= (Blob3.Size < 15)? BLOB3 : 0;

    Blob4.X = data_buf[10];
    Blob4.Y = data_buf[11];
    s   = data_buf[12];
    Blob4.X += (s & 0x30) <<4;
    Blob4.Y += (s & 0xC0) <<2;
    Blob4.Size = (s & 0x0F);

    blobcount |= (Blob4.Size < 15)? BLOB4 : 0;

    return blobcount;
}

